/**
 * @file test_route.c
 * @author sgfz
 * @brief 测试用完整路线
 * @version 0.1
 * @date 2024-09-15
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "test_route.h"
#include "route.h"
#include "reset.h"
#include "posture.h"
#include "trace.h"
#include "bsp_adc.h"
#include "turn.h"
#include "bsp_servo.h"
#include "bsp_sensor.h"
#include "bsp_SysTick.h"

extern int cnt_hdl;
extern int cnt_hdr;

void Route_Door1_Jugde(void)
{
    Tai1_Tai2();
    Tai2_jd2_Tai4();
    Tai4_Tai3();
    Tai3_jd1_door2();
    Stop(100);
    door2_door1();
    Stop(100);
    door1_Tai5();
    Tai5_jd5_Tai7();
    Tai7_Tai8();
    Tai8_jd3();
    jd3_jd4();
    jd4_door1_Home();
    stop();
}

void Route_Door2_Jugde(void)
{
    Tai1_Tai2();
    Tai2_jd2_Tai4();
    Tai4_Tai3();
    Tai3_jd1_door2();
    Stop(100);
    door2_Tai5();
    Tai5_jd5_Tai7();
    Tai7_Tai8();
    Tai8_jd3();
    jd3_jd4();
    jd4_door2_Home();
    stop();
}

void Route_Door3_Jugde(void)
{
    Tai1_Tai2();
    Tai2_jd2_Tai4();
    Tai4_Tai3();
    Tai3_jd1_door2();
    Stop(100);
    door2_door1();
    Stop(100);
    door1_door4();
    Stop(100);
    door4_door3();
    door3_Tai5();
    Tai5_jd5_Tai7();
    Tai7_Tai8();
    Tai8_jd3();
    jd3_jd4();
    jd4_door3_Home();
    stop();
}

void Route_Door4_Jugde(void)
{
    Tai1_Tai2();
    Tai2_jd2_Tai4();
    Tai4_Tai3();
    Tai3_jd1_door2();
    Stop(100);
    door2_door1();
    Stop(100);
    door1_door4();
    Stop(100);
    door4_Tai5();
    Tai5_jd5_Tai7();
    Tai7_Tai8();
    Tai8_jd3();
    jd3_jd4();
    jd4_door4_Home();
    stop();
}